#ifndef TASK_PLANNER__WAYPOINTS_CONFIG_HPP_
#define TASK_PLANNER__WAYPOINTS_CONFIG_HPP_

#include <vector>
#include <map>
#include <string>
#include <yaml-cpp/yaml.h>
#include "rclcpp/rclcpp.hpp"

namespace task_planner
{

// 位置点结构体
struct WayPoint
{
  double x;
  double y;
  double z;
  double yaw;  // 角度（度）
  int32_t number_id;  // 数字点编号（0表示非数字点，1-12表示数字点）
  
  WayPoint() : x(0.0), y(0.0), z(0.0), yaw(0.0), number_id(0) {}
  WayPoint(double x_, double y_, double z_, double yaw_, int32_t number_id_ = 0) 
    : x(x_), y(y_), z(z_), yaw(yaw_), number_id(number_id_) {}
};

// 配置加载器类
class WaypointsConfig
{
public:
  WaypointsConfig() = default;
  
  // 从YAML文件加载配置
  bool loadFromFile(const std::string& config_file_path);
  
  // 获取初始目标点
  const std::vector<WayPoint>& getInitialWaypoints() const { return initial_waypoints_; }
  
  // 获取C区过渡点
  const std::map<std::string, WayPoint>& getCZoneCornerPoints() const { return c_zone_corner_points_; }
  
  // 获取数字位置
  WayPoint getNumberPosition(int32_t number) const;
  
  // 获取路径段配置
  double getTopY() const { return top_y_; }
  double getBottomY() const { return bottom_y_; }
  double getTolerance() const { return tolerance_; }
  
  // 获取默认位置
  const WayPoint& getDefaultPosition() const { return default_position_; }
  
  // 获取C区过渡点数组（按顺序：A, AB_mid, B, C, CD_mid, D）
  std::vector<WayPoint> getCZoneCornerPointsArray() const;

private:
  std::vector<WayPoint> initial_waypoints_;
  std::map<std::string, WayPoint> c_zone_corner_points_;
  std::map<int32_t, WayPoint> number_positions_;
  
  // 路径段配置
  double top_y_ = -1.9;
  double bottom_y_ = -2.67;
  double tolerance_ = 0.1;
  
  // 默认位置
  WayPoint default_position_;
  
  // 辅助函数：从YAML节点解析WayPoint
  WayPoint parseWayPoint(const YAML::Node& node);
};

} // namespace task_planner

#endif // TASK_PLANNER__WAYPOINTS_CONFIG_HPP_
